#include "waypoint.h"
#include <iostream>
using namespace std;

int main()
{
	//defining a first waypoint
	WayPoint wp1;
	wp1.setX(5);
	wp1.setY(6);
	wp1.setTheta(3);
	wp1.setTimeStamp(1);
	wp1.setKick(false);

    //check its porperties
	wp1.Properties();

    //define a second waypoint
	WayPoint wp2;
	wp2.setX(3);
	wp2.setY(11);
	wp2.setTheta(0);
	wp2.setTimeStamp(2);
	wp2.setKick(true);

    //check properties
	wp2.Properties();

	//compute distance between and print
    double dist=wp1.computeDistance(wp2);
    cout<<"\n\tDistance between waypoints: "<<dist<<endl;

    return 0;
}
